#include "sos.h"
#include <malloc.h>

#define TASK_COUNT 5

extern TCB* gl_current_task;

//Task queue
TCB* gl_task_queue[TASK_COUNT];

void _save_registers(void) _naked;
void _save_stack(void) _naked;
void _restore_stack(void) _naked;
void _restore_registers(void) _naked;

/* */
void task_create(unsigned char id, void (*task_fn_ptr)(void)) critical
{
		TCB* new_task = (TCB*) malloc(sizeof(TCB));
		new_task->id = id;
		new_task->pxstack = (xdata unsigned char*) malloc(256);
		new_task->task_state = TS_READY;
		new_task->pxstack_start = new_task->pxstack;
		new_task->task_fn_ptr = task_fn_ptr;
		//Add task to the task queue
		gl_task_queue[id - 1] = new_task;

}
/*
 * Find the next task in the READY state. If there are
 * no user tasks available, return the kernel task. 
 */
unsigned char next_ready_task()
{
	unsigned char proc_id = (gl_current_task->id % TASK_COUNT) + 1;
	while(1)
	{
		//If the next task is ready,
		if(gl_task_queue[proc_id]->task_state == TS_READY)
			//send it
			return proc_id;
		else if (proc_id == 0)//0 is the kernel task
			proc_id++;
		else if (proc_id = gl_current_task->id + 1) //OK we've come full circle.
			return 0;
		else //you'll hopefully hit one of the return statements !!
			proc_id = (proc_id % TASK_COUNT) + 1;
	}
}
//A task can be suspended pending a semaphore wait
//How do you suspend a task? just mark it suspend. 
//Find the next task that is READY and run that task
//Whenever a semaphore becomes free, call a fn. that 
//checks to see if there is any task waiting on that 
//semaphore and wake that task.
//Scenario: A serial interrupt occured. 
//Action Taken: Suspend current task? Do you have to?
//Make sure you push the reqd. variables on the stack 
void suspend(void) _naked
{
	_save_registers();
	_save_stack();
	//Set this process state to READY
	gl_current_task->task_state = TS_SUSPEND;
	//Look for next READY task
	//OK Some regs are going to get messed up here,
	//but we don't care since we've saved them
	gl_current_task = gl_task_queue[next_ready_task()];
	_restore_stack();
	_restore_registers(); //calls reti.
}

void preempt(void) _naked
{
	_save_registers();
	_save_stack();
	//Set this process state to READY
	gl_current_task->task_state = TS_READY;
	//Look for next READY task
	//OK Some regs are going to get messed up here,
	//but we don't care since we've save them
	gl_current_task = gl_task_queue[next_ready_task()];
	_restore_stack();
	_restore_registers(); //calls reti.
}

//Whenever a semaphore is free, make all processes 
//suspended on that semaphore ready. The rr scheduler
//will pick up the task in its normal course of action.
void resume(semaphore sem_address)
{
	int i=0;
	for(i=0; i<TASK_COUNT; i++)
	{
		if(gl_task_queue[i]->sem_address == sem_address)
			gl_task_queue[i]->task_state = TS_READY;
	}
}

void _save_registers(void) _naked
{
		_asm																						
			push	acc
			push	b
			push	dpl
			push	dph
			push	(0+2)
			push	(0+3)
			push	(0+4)
			push	(0+5)
			push	(0+6)
			push	(0+7)
			push	(0+0)
			push	(0+1)
			push	psw
			mov psw,#0x00
			push	_bp																				
	_endasm;		
}

void _restore_registers(void) _naked
{
	_asm	
		pop _bp			
		pop	psw
		pop	(0+1)
		pop	(0+0)
		pop	(0+7)
		pop	(0+6)
		pop	(0+5)
		pop	(0+4)
		pop	(0+3)
		pop	(0+2)
		pop	dph
		pop	dpl
		pop	b
		pop	acc
		reti																					
	_endasm;																					
}

/*
 * Writes stack to external memory
 */
void _save_stack(void) _naked
{
	/* stack grows upward. SP always points to the next empty location.
	 * Eg. If stack starts at 0x007, and there is one element on the 
	 * stack, SP points to 0x008. 
	 */
	//gl_current_task->pxstack_start -=  (SP - 1) - gl_start_of_stack;
	gl_current_task->pxstack = gl_current_task->pxstack_start;
	while(--SP >= gl_start_of_stack)
	{
		*(gl_current_task->pxstack++) = DEREF(SP);
	}
}
 
void _restore_stack(void) _naked
{
	SP = gl_start_of_stack;
	while(gl_current_task->pxstack-- != gl_current_task->pxstack_start)
	{
		DEREF(SP) = *(gl_current_task->pxstack);
		//(DEREF(SP))++;
		SP++;
	}
	//No return statement reqd.

}



